
#include <math.h>
#include "Vector3d.h"
#include "Quaternion.h"

Quaternion Quaternion_fromValues( double w, double x, double y, double z )
{
	Quaternion q;
	q.w = w;
	q.x = x;
	q.y = y;
	q.z = z;
	return q;
};
Quaternion Quaternion_fromAxisAngle( Vector3d* v, double angle )
{
	Quaternion q;
	double s;
	angle /= 2;
	s = sin(angle);
	q.w = cos(angle);
	q.x = s * v->x;
	q.y = s * v->y;
	q.z = s * v->z;
	return q;
};

Quaternion Quaternion_fromAxisAngleValues( double x, double y, double z, double angle )
{
	Quaternion q;
	double s;
	angle /= 2;
	s = sin(angle);
	q.w = cos(angle);
	q.x = s * x;
	q.y = s * y;
	q.z = s * z;
	return q;
};

void Quaternion_toAxisAngle( Quaternion* q, Vector3d* v, double* angle )
{
	double m = sqrt( 1 - (q->w * q->w) );
	*angle = 2 * acos(q->w);
	if( m==0 )
	{
		v->x = q->x;
		v->y = q->y;
		v->z = q->z;
	}
	else
	{
		v->x = q->x/m;
		v->y = q->y/m;
		v->z = q->z/m;
	}
};

Quaternion Quaternion_multiply( Quaternion* p, Quaternion* q )
{
	Quaternion r;
	r.w = (p->w * q->w) - (p->x * q->x) - (p->y * q->y) - (p->z * q->z);
	r.x = (p->x * q->w) + (p->w * q->x) - (p->z * q->y) + (p->y * q->z);
	r.y = (p->y * q->w) + (p->z * q->x) + (p->w * q->y) - (p->x * q->z);
	r.z = (p->z * q->w) - (p->y * q->x) + (p->x * q->y) + (p->w * q->z);
	return r;
};

Quaternion Quaternion_normalized( Quaternion* q )
{
	Quaternion r;
	double magnitude = sqrt( (q->x*q->x) + (q->y*q->y) + (q->z*q->z) + (q->w*q->w) );
	r.w = q->w / magnitude;
	r.x = q->x / magnitude;
	r.y = q->y / magnitude;
	r.z = q->z / magnitude;
	return r;
};

/*
 --- FOR SAFE KEEPING ---
 void quatToMatrix( Quaternion* q )
 {
 m[0]  = 1 - 2*(q->y*q->y + q->z*q->z);
 m[1]  =		2*(q->x*q->y - q->z*q->w);
 m[2]  =		2*(q->x*q->z + q->y*q->w);
 m[3]  =		0;

 m[4]  =		2*(q->x*q->y + q->z*q->w);
 m[5]  = 1 -	2*(q->x*q->x + q->z*q->z);
 m[6]  =		2*(q->y*q->z - q->x*q->w);
 m[7]  =		0;

 m[8]  =		2*(q->x*q->z - q->y*q->w);
 m[9]  =		2*(q->y*q->z + q->x*q->w);
 m[10] = 1 - 2*(q->x*q->x + q->y*q->y);
 m[11] =		0;

 m[12] =		0;
 m[13] =		0;
 m[14] =		0;
 m[15] =		1;

 glMultMatrixd(m);
 }*/